#include "projector.h"
#include "sharedStructure.hpp"
#include <math.h>
#include <stdio.h>
#include <stdlib.h>
#include <string.h>
#include <assert.h>
#include <mex.h>



bool projector::initialize(float s, float *p_R, float *p_T,
				int num_orient, int num_length, int filter_size){
				// s
				this->s_wt = s;
				this->inv_s_wt = 1.0f/s;
				// R
				for( int r = 0; r<3; ++r){
					for (int c = 0; c<3; ++c){
						R[r][c] = p_R[c*3+r];
					}
				}
				// compute inv(R);
				invR();
				
				// T, 

				memcpy( this->T,p_T,sizeof(float)*3);
				
				//
				n_orient = num_orient;
				n_length = num_length;
				
				this->fh = filter_size;
                
                 cache_cos= (float*)malloc(num_orient*sizeof(float));
                 cache_sin = (float*)malloc(num_orient*sizeof(float));
                for( int i_orient = 0; i_orient<num_orient; ++i_orient){
                    cache_cos[i_orient]=cos((PI*i_orient)/num_orient);
                    cache_sin[i_orient]=sin((PI*i_orient)/num_orient);
                }


				return true;

}

bool projector::release(){

    free(cache_cos);
    free(cache_sin);
	return true;
}


				
void projector::invR(){
	float det = R[0][0]*R[1][1]*R[2][2] + R[0][1]*R[1][2]*R[2][0] + R[1][0]*R[2][1]*R[0][2]
			  - R[0][2]*R[1][1]*R[2][0] - R[0][0]*R[1][2]*R[2][1] - R[0][1]*R[1][0]*R[2][2];
	assert(fabs(det)>1e-10);
	inv_R[0][0]=R[1][1]*R[2][2]-R[1][2]*R[2][1];
	inv_R[1][0]=R[1][2]*R[2][0]-R[1][0]*R[2][2];
	inv_R[2][0]=R[1][0]*R[2][1]-R[1][1]*R[2][0];
	
	inv_R[0][1]=R[0][2]*R[2][1]-R[0][1]*R[2][2];
	inv_R[1][1]=R[0][0]*R[2][2]-R[0][2]*R[2][0];
	inv_R[2][1]=R[0][1]*R[2][0]-R[0][0]*R[2][1];
	
	inv_R[0][2]=R[0][1]*R[1][2]-R[0][2]*R[1][1];
	inv_R[1][2]=R[0][2]*R[1][0]-R[0][0]*R[1][2];
	inv_R[2][2]=R[0][0]*R[1][1]-R[0][1]*R[1][0];
	
	det = 1.0f/det;
	for (int r=0; r<3; ++r){
		for(int c = 0; c<3; ++c){
			inv_R[r][c]*=det;
		}
	}
	return;
}

bool projector::updatePlane(float *p_u, float *p_v, float *p_ori, float *p_nt, float s, float width, float height){
	memcpy(u,p_u, sizeof(float)*3);
	memcpy(v,p_v, sizeof(float)*3);
	float norm = u[0]*u[0]+u[1]*u[1]+u[2]*u[2];
	if( fabs(norm-1)>1e-6){
		printf("input bu is not unit vector: norm(bu)=%f\n",norm);
		u[0]/=norm;
		u[1]/=norm;
		u[2]/=norm;
	}
	
	norm = v[0]*v[0]+v[1]*v[1]+v[2]*v[2];
	if( fabs(norm-1)>1e-6){
		printf("input bv is not unit vector: norm(bv)=%f\n",norm);
		v[0]/=norm;
		v[1]/=norm;
		v[2]/=norm;
	}
	
	memcpy(nt,p_nt,sizeof(float)*4);
	norm = nt[0]*nt[0]+nt[1]*nt[1]+nt[2]*nt[2];
	if( fabs(norm-1)>1e-6){
		printf("input nt is not unit vector: norm(nt)=%f\n",norm);
		nt[0]/=norm;
		nt[1]/=norm;
		nt[2]/=norm;
		nt[3]/=norm;
	}
	
	// convert the nt from tan coordiante system to world coordinate system
	tanDirection2World(nt[0],nt[1],nt[2],n[0],n[1],n[2]);
	
	memcpy(plane_origin, p_ori, sizeof(float)*3);
	
	this->s_tp = s;
	this->inv_s_tp = 1.0f/s;
	this->w = width; // size in plane coordinate unit, e.g. pixels
	this->h = height;
	
	return true;
}	

bool projector::updateCamera(float *pP, float *direction, float zoom_x, float zoom_y){
	for( int r = 0; r<3; ++r){
		for (int c = 0; c<4; ++c){
			P[r][c]= pP[r+c*3];
		}
	}
	
	memcpy(cam_dir,direction,sizeof(float)*3);
	float norm = direction[0]*direction[0]+direction[1]*direction[1]+direction[2]*direction[2];
	if( fabs(norm-1)>1e-10){
		cam_dir[0]/=norm;
		cam_dir[1]/=norm;
		cam_dir[2]/=norm;
	}
	
	zx = zoom_x;
	zy = zoom_y;

	updatePreprocessedMatrix();

	return true;
}


bool projector::tanPoint2World(float Xt, float Yt, float Zt, float &Xw, float &Yw, float &Zw){
	// X^T = s*R*(X^W+T);
	// X^W = inv(s)inv(R) X^T -T
	Xw = (inv_R[0][0]*Xt+inv_R[0][1]*Yt+inv_R[0][2]*Zt)*inv_s_wt - T[0];
	Yw = (inv_R[1][0]*Xt+inv_R[1][1]*Yt+inv_R[1][2]*Zt)*inv_s_wt - T[1];
	Zw = (inv_R[2][0]*Xt+inv_R[2][1]*Yt+inv_R[2][2]*Zt)*inv_s_wt - T[2];
	return true;
}


bool projector::tanDirection2World(float Xt, float Yt, float Zt, float &Xw, float &Yw, float &Zw){
	// X^T = s*R*(X^W+T);
	// X^W = inv(s)inv(R) X^T -T
	Xw = (inv_R[0][0]*Xt+inv_R[0][1]*Yt+inv_R[0][2]*Zt);
	Yw = (inv_R[1][0]*Xt+inv_R[1][1]*Yt+inv_R[1][2]*Zt);
	Zw = (inv_R[2][0]*Xt+inv_R[2][1]*Yt+inv_R[2][2]*Zt);
	return true;
}



bool projector::planePoint2Tan(float a, float b, float &Xt, float &Yt, float &Zt){
	Xt = plane_origin[0]+(a*u[0]+b*v[0])*inv_s_tp;
	Yt = plane_origin[1]+(a*u[1]+b*v[1])*inv_s_tp;
	Zt = plane_origin[2]+(a*u[2]+b*v[2])*inv_s_tp;
	return true;
}


bool projector::worldPoint2Image(float X, float Y, float Z, float &x, float &y){
	
	x = P[0][0]*X+P[0][1]*Y+P[0][2]*Z+P[0][3];
	y = P[1][0]*X+P[1][1]*Y+P[1][2]*Z+P[1][3];
	float z = P[2][0]*X+P[2][1]*Y+P[2][2]*Z+P[2][3];
	x = x/z*zy; // zx zy is in matlab coordinate system
	y = y/z*zx; // zx zy is in matlab coordinate system
	// note that here, origin is at the center of the image
	return true;
}



bool projector::planePoint2Image(float xp, float yp, float&x, float&y){;
	float zt;
	/*PRW constT*/
	x = PRW[0]*xp+PRW[3]*yp+constT[0];
	y = PRW[1]*xp+PRW[4]*yp+constT[1];
	zt = PRW[2]*xp+PRW[5]*yp+constT[2];
	
	x = x/zt*zy;
	y = y/zt*zx;
	//printf("fast method: x:%f, y:%f,z:%f\n",x,y,zt);
	return true;
}


bool projector::planePoint2ImageSlow(float xp, float yp, float &x, float &y){
	float Xt,Yt,Zt;
	float Xw,Yw,Zw;

	//planePoint2Tan(xp,yp,Xt,Yt,Zt);
	Xt = plane_origin[0]+(xp*u[0]+yp*v[0])*inv_s_tp;
	Yt = plane_origin[1]+(xp*u[1]+yp*v[1])*inv_s_tp;
	Zt = plane_origin[2]+(xp*u[2]+yp*v[2])*inv_s_tp;

	//tanPoint2World(Xt,Yt,Zt,Xw,Yw,Zw);
	Xw = (inv_R[0][0]*Xt+inv_R[0][1]*Yt+inv_R[0][2]*Zt)*inv_s_wt - T[0];
	Yw = (inv_R[1][0]*Xt+inv_R[1][1]*Yt+inv_R[1][2]*Zt)*inv_s_wt - T[1];
	Zw = (inv_R[2][0]*Xt+inv_R[2][1]*Yt+inv_R[2][2]*Zt)*inv_s_wt - T[2];


	//worldPoint2Image(Xw,Yw,Zw,x,y);

	x = P[0][0]*Xw+P[0][1]*Yw+P[0][2]*Zw+P[0][3];
	y = P[1][0]*Xw+P[1][1]*Yw+P[1][2]*Zw+P[1][3];
	float z = P[2][0]*Xw+P[2][1]*Yw+P[2][2]*Zw+P[2][3];
	x = x/z*zy; // zx zy is in matlab coordinate system
	y = y/z*zx; // zx zy is in matlab coordinate system
	// note that here, origin is at the center of the image
	//printf("slow method: x:%f, y:%f,z:%f\n",x,y,z);
	return true;
}

bool projector::planeLine2Image(const line &plane_line, line &matlab_line){
	printf("warning, using deprecated function\n");
	
	float s_x,s_y; // start x, y, in Plane coordinate
	float e_x,e_y;
	
	//float theta = PI*((float)(plane_line.orient)/(n_orient));
    float theta;
	int o = plane_line.orient;
	float len =plane_line.length*0.5f;
	s_x = plane_line.x - len*cache_sin[o];
	s_y = plane_line.y + len*cache_cos[o];
	
	e_x = plane_line.x + len*cache_sin[o];
	e_y = plane_line.y - len*cache_cos[o];
	
	float s_x_i,s_y_i;
	float e_x_i,e_y_i;
	
	planePoint2Image(s_x,s_y,s_x_i,s_y_i);
	
    planePoint2Image(e_x,e_y,e_x_i,e_y_i);


	len = (s_y_i-e_y_i)*(s_y_i-e_y_i)+(s_x_i-e_x_i)*(s_x_i-e_x_i);
	len = sqrt(len);
	
	
	theta = atan((-e_y_i+s_y_i)/(e_x_i-s_x_i)); // this is in image coordinate
	if( fabs(e_x_i - s_x_i)<1e-8)
		theta = PI/2;
	if( theta<0){ theta = theta + PI; }

	matlab_line.x = ROUND(0.5f*(e_x_i+s_x_i)); // this result is in image coordinate,
	matlab_line.y = ROUND(0.5f*(e_y_i+s_y_i));// this result is in image coordinate
	
	matlab_line.orient = floor(theta/PI*n_orient);
	matlab_line.length = len;
	
	return true;
}



bool projector::planeLineVertex2MatlabImage(const lineVertex &plane_line, lineVertex &matlab_line){

	//planePoint2Image(s_x,s_y,s_x_i,s_y_i);
	planePoint2Image(plane_line.x_start,plane_line.y_start, matlab_line.y_start,matlab_line.x_start);

   // planePoint2Image(e_x,e_y,e_x_i,e_y_i);
	planePoint2Image(plane_line.x_end,plane_line.y_end, matlab_line.y_end,matlab_line.x_end);
	
	return true;
}



// decide if current plane is visible under current camera;
bool projector::isVisible(){
	float dot = n[0]*cam_dir[0]+n[1]*cam_dir[1]+n[2]*cam_dir[2];
	return dot< -0.4f;
}



void projector::updatePreprocessedMatrix(){

	/*
	FILE* pFile = fopen("./matrix.txt","w");
	fprintf(pFile,"inv_R:\n");
	for( int i = 0; i<3; ++i){
		for(int j=0; j<3; ++j){
			fprintf(pFile,"%f,",inv_R[i][j]);
		}
		printf("\n");
	}

fprintf(pFile,"inv_s_tp,%f, \t inv_s_wt, %f\n",inv_s_tp,inv_s_wt);
fprintf(pFile,"po: %f,%f,%f\n",plane_origin[0],plane_origin[1],plane_origin[2]);

	printf("P:\n");
	for( int i = 0; i<3; ++i){
		for(int j=0; j<4; ++j){
			fprintf(pFile,"%f,",P[i][j]);
		}
		fprintf(pFile,"\n");
	}
	*/

	/*W*/
	PRW[0]=u[0];PRW[3]=v[0];
	PRW[1]=u[1];PRW[4]=v[1];
	PRW[2]=u[2];PRW[5]=v[2];
	//fprintf(pFile,"uv:\n %f,%f,\n %f,%f,\n %f,%f\n",PRW[0],PRW[1],PRW[2],PRW[3],PRW[4],PRW[5]);

	/*inv_R*W*/
	float temp[6];
	temp[0]=inv_R[0][0]*PRW[0]+inv_R[0][1]*PRW[1]+inv_R[0][2]*PRW[2];
	temp[1]=inv_R[1][0]*PRW[0]+inv_R[1][1]*PRW[1]+inv_R[1][2]*PRW[2];
	temp[2]=inv_R[2][0]*PRW[0]+inv_R[2][1]*PRW[1]+inv_R[2][2]*PRW[2];
	
	temp[3]=inv_R[0][0]*PRW[3]+inv_R[0][1]*PRW[4]+inv_R[0][2]*PRW[5];
	temp[4]=inv_R[1][0]*PRW[3]+inv_R[1][1]*PRW[4]+inv_R[1][2]*PRW[5];
	temp[5]=inv_R[2][0]*PRW[3]+inv_R[2][1]*PRW[4]+inv_R[2][2]*PRW[5];

	//fprintf(pFile,"Temp:\n %f,%f,\n %f,%f,\n %f,%f\n",temp[0],temp[1],temp[2],temp[3],temp[4],temp[5]);




	/*P33*inv_R*W*/
	PRW[0] = P[0][0]*temp[0]+P[0][1]*temp[1]+P[0][2]*temp[2];
	PRW[1] = P[1][0]*temp[0]+P[1][1]*temp[1]+P[1][2]*temp[2];
	PRW[2] = P[2][0]*temp[0]+P[2][1]*temp[1]+P[2][2]*temp[2];

	PRW[3] = P[0][0]*temp[3]+P[0][1]*temp[4]+P[0][2]*temp[5];
	PRW[4] = P[1][0]*temp[3]+P[1][1]*temp[4]+P[1][2]*temp[5];
	PRW[5] = P[2][0]*temp[3]+P[2][1]*temp[4]+P[2][2]*temp[5];
	/*alpha*beta*P33*inv_R*W*/
	float ab = inv_s_tp*inv_s_wt;
	for(int i =0; i<6; ++i)	PRW[i]*=ab;

	//printf("PRW[5]:%f,!!!!!!!!!!!\n",PRW[5]);
	//fprintf(pFile,"PRW:\n %f,%f,\n %f,%f,\n %f,%f\n",PRW[0],PRW[1],PRW[2],PRW[3],PRW[4],PRW[5]);

	/*constT*/
	/*constT=inv_R*po*/
	constT[0]=inv_R[0][0]*plane_origin[0]+inv_R[0][1]*plane_origin[1]+inv_R[0][2]*plane_origin[2];
	constT[1]=inv_R[1][0]*plane_origin[0]+inv_R[1][1]*plane_origin[1]+inv_R[1][2]*plane_origin[2];
	constT[2]=inv_R[2][0]*plane_origin[0]+inv_R[2][1]*plane_origin[1]+inv_R[2][2]*plane_origin[2];
	
	/**beta*inv_R*po-T*/
	for( int i =0; i<3; ++i)
		constT[i]=constT[i]*inv_s_wt-T[i];
	
	/**P33(beta*inv_R*po-T)*/
	temp[0] = P[0][0]*constT[0]+P[0][1]*constT[1]+P[0][2]*constT[2];
	temp[1] = P[1][0]*constT[0]+P[1][1]*constT[1]+P[1][2]*constT[2];
	temp[2] = P[2][0]*constT[0]+P[2][1]*constT[1]+P[2][2]*constT[2];

	/**P31+P33(beta*inv_R*po-T)*/
	for( int i =0; i<3; ++i)
		constT[i]=P[i][3]+temp[i];

	/*
	fprintf(pFile,"T:\n %f,%f,%f,\n",T[0],T[1],T[2]);
	fprintf(pFile,"constT:\n %f,%f,%f,\n",constT[0],constT[1],constT[2]);
	fclose(pFile);
	*/
}

